Controlling the Servo and Motors
Elias Groot
Software Lead, Course Organizer
To actually spin the motors and servo, you will need to write data to the official ASE actuator service. Based on our trajectory midpoint heuristics, we will write a steering value.
Creating a Write Stream
Similar to reading from the imaging service with a read stream, we want to write to the actuator service with a write stream. To understand to which stream to write, we turn to the actuator service's service.yaml definition and see which inputs it reads from:
...
inputs:
- service: controller
streams:
- decision
...
From the service "controller" it wants the stream "decision". Let's initialize this stream in our code, right after initializing the imaging read stream.
write_stream *actuator = get_write_stream(&service, "decision");
if (actuator == NULL) {
printf("Could not get write stream for 'decision'\n");
return 1;
}
Notice that here we do not specify to which service we want to write to. We just tell the roverlib that "I want to write some data to a stream named 'decision'". We do not know nor check who will actually read from this stream.
Again similar to our imaging read stream, we have two methods available for writing data:
write_pb()which will take aSensorOutputmessage as Go struct, encode it to the binary wire format and send it outwrite_bytes()which will take a raw write buffer from the caller and send it out
Writing To a Write Stream
Because we know that the actuator service expects a ControllerOutput message, using the actuator.Write() message is the obvious choice. Take a look at its definition here to understand which properties can be set for the actuator.
For now, let's only set the servo position, without spinning up the motors. To do so, we construct a ControllerOutput message first.
// Initialize the message that we want to send to the controller
ProtobufMsgs__SensorOutput actuator_msg = PROTOBUF_MSGS__SENSOR_OUTPUT__INIT;
// Set the message fields
msg->timestamp = current_time_millis(); // milliseconds since epoch
msg->status = 0; // all is well
msg->sensorid = 1; // this is the first and only sensor we have
// Set the oneof field contents
ProtobufMsgs__ControllerOutput controller_output = PROTOBUF_MSGS__CONTROLLER_OUTPUT__INIT;
controller_output.steeringangle = steerPosition;
controller_output.leftthrottle = 0;
controller_output.rightthrottle = 0;
controller_output.fanspeed = 0;
controller_output.frontlights = false;
// Set the oneof field (union)
msg->controlleroutput = &controller_output;
msg->sensor_output_case = PROTOBUF_MSGS__SENSOR_OUTPUT__SENSOR_OUTPUT_CONTROLLER_OUTPUT;
The union type and case setup might seem complex at first sight. It unfortunately is a result of the C typing system to allow for (somewhat complicated) union types. Also notice that we manually specify three general fields:
timestamp: when was this message generated? (In milliseconds since epoch)status: is this "sensor" still working well? (0 = all is well)sensorId: which sensor in this service did create this message? Must be non-empty. Useful if you have multiple sensors in the same sensor (for example, two cameras in one imaging service).
To send this SensorOutput message, we just use write_pb() like so:
// Send the message to the actuator
int res = write_pb(actuator, &actuator_msg);
if (res != 0) {
printf("Could not write to actuator\n");
return 1;
}
Convenient right? If we keep writing in a loop, we will continuously update our steering decisions that the actuator service will use to spin the servo and motors. At this point, your code should look like the following:
#include <roverlib.h>
#include <sys/time.h>
#include <unistd.h>
long long current_time_millis() {
struct timeval tv;
gettimeofday(&tv, NULL);
return (long long)(tv.tv_sec) * 1000 + (tv.tv_usec) / 1000;
}
// The main user space program
// this program has all you need from roverlib: service identity, reading, writing and configuration
int user_program(Service service, Service_configuration *configuration) {
// "I am the dummy controller, and I want to read from the 'path' stream from the 'imaging' service"
read_stream *imaging = get_read_stream(&service, "imaging", "path");
if (imaging == NULL) {
printf("Could not get read stream for 'path' from 'imaging'\n");
return 1;
}
write_stream *actuator = get_write_stream(&service, "decision");
if (actuator == NULL) {
printf("Could not get write stream for 'decision'\n");
return 1;
}
while (true) {
// Read one message from the stream
ProtobufMsgs__SensorOutput *msg = read_pb(imaging);
if (msg == NULL) {
printf("Could not read from input example-input\n");
}
// When did imaging service create this message?
int createdAt = msg->timestamp;
printf("Received message with timestamp: %d\n", createdAt);
// Get the camera data
if (msg->sensor_output_case != PROTOBUF_MSGS__SENSOR_OUTPUT__SENSOR_OUTPUT_CAMERA_OUTPUT) {
printf("Received sensor data, but not camera data\n");
} else {
ProtobufMsgs__CameraSensorOutput *camera_output = msg->cameraoutput;
printf("Imaging service captured a %d by %d image\n", camera_output->trajectory->width, camera_output->trajectory->height);
// This value holds the steering position that we want to pass to the servo (-1 = left, 0 = center, 1 = right)
float steerPosition = 0;
// Find the desired mid point (x) of the captured image
int desiredMidpoint = camera_output->trajectory->width / 2;
// Compute the actual mid point between the left and right edges of the detected track
if (camera_output->trajectory->n_points < 2) {
printf("Not enough track edge points to compute the mid point\n");
} else {
// Compute the mid point (assuming [0] is the left edge and [1[] is the right edge)
int actualMidpoint = (camera_output->trajectory->points[1]->x - camera_output->trajectory->points[0]->x) / 2 + camera_output->trajectory->points[0]->x;
// Compute the error
int midpointErr = actualMidpoint - desiredMidpoint;
// Compute the steering position
steerPosition = (float)midpointErr / (float)desiredMidpoint;
// Initialize the message that we want to send to the controller
ProtobufMsgs__SensorOutput actuator_msg = PROTOBUF_MSGS__SENSOR_OUTPUT__INIT;
// Set the message fields
msg->timestamp = current_time_millis(); // milliseconds since epoch
msg->status = 0; // all is well
msg->sensorid = 1; // this is the first and only sensor we have
// Set the oneof field contents
ProtobufMsgs__ControllerOutput controller_output = PROTOBUF_MSGS__CONTROLLER_OUTPUT__INIT;
controller_output.steeringangle = steerPosition;
controller_output.leftthrottle = 0;
controller_output.rightthrottle = 0;
controller_output.fanspeed = 0;
controller_output.frontlights = false;
// Set the oneof field (union)
msg->controlleroutput = &controller_output;
msg->sensor_output_case = PROTOBUF_MSGS__SENSOR_OUTPUT__SENSOR_OUTPUT_CONTROLLER_OUTPUT;
// Send the message to the actuator
int res = write_pb(actuator, &actuator_msg);
if (res != 0) {
printf("Could not write to actuator\n");
return 1;
}
}
}
}
}
// This is just a wrapper to run the user program
// it is not recommended to put any other logic here
int main() {
return run(user_program);
}
This already makes for a fully function (though quite dumb) controller service. Though, just like with declaring our inputs, we need to declare our output stream decision to roverd again through the service.yaml definition.
Declaring a Write Stream
Declaring a write stream through the service.yaml is even easier than declaring a read stream. You just need to specify the name of your stream in the outputs field, like so:
...
outputs:
- decision
...
Before you save, there is one more thing that we need to do. If you look at the actuator's service.yaml definition, you will see that it depends on stream decision from a service called controller. roverd will find and match this stream and service based on exact naming. Chances are that your service is currently named differently, so again turn to your service.yaml and make sure to adjust the name like so:
name: controller
...
This is all. Save all your changes and use roverctl to sync them if your Rover is powered on.